/** 
  需求：编写动作客户端实现，可以提交一个整型数据到服务端，并处理服务端的连续反馈以及最终返回结果。
  步骤：
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.创建动作客户端；
      3-2.发送请求；
      3-3.处理目标发送后的反馈；
      3-4.处理连续反馈；
      3-5.处理最终响应。
    4.调用spin函数，并传入节点对象指针；
    5.释放资源。
*/

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces_demo/action/progress.hpp"

using base_interfaces_demo::action::Progress;
using GoalHandleProgress = rclcpp_action::ClientGoalHandle<Progress>;
using namespace std::placeholders;

// 3. 定义节点类
class MinimalActionClient : public rclcpp::Node
{
public:
    explicit MinimalActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
    : Node("minimal_action_client", node_options)
    {
        // 3-1 创建动作客户端
        this->client_ptr_ = rclcpp_action::create_client<Progress>(this, "get_sum");
    }

    // 发送请求
    // 发送目标请求 send_goal 函数发送目标给服务器，目标值是 num，传递给服务器用于求和。
    void send_goal(int64_t num)
    {
        if(!this->client_ptr_)
        {
            RCLCPP_ERROR(this->get_logger(), "动作客户端未被初始化。");
        }

        // 等待服务器响应：wait_for_action_server 确保客户端可以连接到服务器
        if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10)))
        {
            RCLCPP_ERROR(this->get_logger(), "服务连接失败！");
            return;
        }
        
        auto goal_msg = Progress::Goal();
        goal_msg.num = num;
        RCLCPP_INFO(this->get_logger(), "发送请求数据！");

        // 这行代码创建了一个 SendGoalOptions 对象，用于配置和管理发送动作目标请求时的选项和回调函数
        auto send_goal_options = rclcpp_action::Client<Progress>::SendGoalOptions();
        /**
         * 1. 目标发送后的响应(服务器接收到目标后的反馈)
         * 2. 连续的进度反馈
         * 3. 最终结果的处理
         */

        // 是否接收客户端发送的目标请求(即动作请求)
        send_goal_options.goal_response_callback 
        = std::bind(&MinimalActionClient::goal_response_callback, this, _1);

        // 处理服务器发送的连续进度反馈
        send_goal_options.feedback_callback 
        = std::bind(&MinimalActionClient::feedback_callback, this, _1, _2);
        
        // 用于处理动作执行完毕时的最终结果
        send_goal_options.result_callback
        = std::bind(&MinimalActionClient::result_callback, this, _1);
        

        // 通过ROS2动作客户端异步发送一个目标(goal)请求，并返回一个future对象，用于异步等待服务器的响应
        auto goal_handle_future = this->client_ptr_->async_send_goal(goal_msg, send_goal_options);  
        /** 关键点
         *  1. 异步发送目标
         *     - 该方法是异步的，不会立即等待服务器的响应，而是继续执行后面的代码。服务器接收目标的响应将在回调函数(如 goal_response_callback)中处理
         *  2. future对象
         *     - goal_handle_future 是一个future，它允许你在稍后使用get()来获取目标的处理状态，或者通过回调函数处理目标的执行和反馈
         * */ 
    }

    /**
     * 过程总结
     * 1. 客户端通过async_send_goal方法异步发送目标请求给动作服务器
     * 2. 同时，客户端注册了几个回调函数来处理服务器的响应、进度反馈和最终结果。
     * 3. goal_handle_future是一个future对象，它会在目标被服务器接收时被填充，允许客户端在异步模式下处理目标请求。
     */
    
private:
    
    // 动作客户端创建，动作客户端的指针，使用create_client<Progress>函数创建，用来与动作服务器通信
    rclcpp_action::Client<Progress>::SharedPtr client_ptr_;    

    // 3-3 处理目标发送后的反馈——服务器收到请求时调用，确定是否接受目标
    void goal_response_callback(GoalHandleProgress::SharedPtr goal_handle)
    {
        if(!goal_handle)
        {
            RCLCPP_ERROR(this->get_logger(), "目标请求被服务器拒绝！");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "目标被接收，等待结果中");
        }
    }

    // 3-4 处理连续反馈——收到服务器的进度反馈时调用，显示百分比进度
    void feedback_callback(GoalHandleProgress::SharedPtr, const std::shared_ptr<const Progress::Feedback> feedback)
    {
        int32_t progress = (int32_t)(feedback->progress * 100);
        RCLCPP_INFO(this->get_logger(), "当前进度: %d%%", progress);
    }

    // 3-5 处理最终响应——当服务器完成任务时调用，显示最终的求和结果
  void result_callback(const GoalHandleProgress::WrappedResult & result)
  {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "任务被中止");
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(this->get_logger(), "任务被取消");
        return;
      default:
        RCLCPP_ERROR(this->get_logger(), "未知异常");
        return;
    }

    RCLCPP_INFO(this->get_logger(), "任务执行完毕，最终结果: %ld", result.result->sum);

  }
};


int main(int argc, char ** argv)
{
    // 2. 初始化ROS2客户端
    rclcpp::init(argc, argv);

    // 调用spin函数，并传入节点对象指针
    auto action_client = std::make_shared<MinimalActionClient>();    // 创建对象并发送请求
    action_client->send_goal(10);
    rclcpp::spin(action_client);    // 使用rclcpp::spin保持节点运行，持续监听和处理来自订阅、服务、动作、定时器等的回调函数
    /**
     * 当调用rclcpp::spin(node)时，ROS2会进入一个循环，在这个循环里，它会：
     * - 处理发布者的消息传递给订阅者的回调函数
     * - 处理服务请求或响应的回调函数
     * - 处理动作客户端或服务器的回调函数
     * - 处理定时器触发的事件
     */
    // 5.释放资源
    rclcpp::shutdown();
    return 0;
}



